/*----------------------------------------------------------------------------
 *      RL-ARM - RTX
 *----------------------------------------------------------------------------
 *      Name:    BLINKY.C
 *      Purpose: RTX example program
 *----------------------------------------------------------------------------
 *      This code is part of the RealView Run-Time Library.
 *      Copyright (c) 2006-2013 KEIL - An ARM Company. All rights reserved.
 *---------------------------------------------------------------------------*/
#include "MK20D5.h"                     // Device header

#define LED_NUM     3                   /* Number of user LEDs                */
const uint32_t led_mask[] = {1UL << 4, 1UL << 3, 1UL << 2};

#define LED_GREEN  0
#define LED_RED    1
#define LED_BLUE   2
#define LED_A      0
#define LED_B      1
#define LED_C      2
#define LED_D      3
#define LED_CLK    4

/*----------------------------------------------------------------------------
  Function that initializes LEDs
 *----------------------------------------------------------------------------*/
void LED_Initialize(void) {

  SIM->SCGC5   |= (1UL << 9) | (1UL << 11) | (1UL << 12); /* Enable Clock to Port A,C,D */ 
  PORTD->PCR[4]  = (1UL <<  8);                           /* PTD.4 is GPIO */
  PORTC->PCR[3]  = (1UL <<  8);                           /* PTC.3 is GPIO */
  PORTA->PCR[2]  = (1UL <<  8);                           /* PTA.2 is GPIO */

  /* Switch LEDs off */
  PTD->PDOR = led_mask[0];
  PTC->PDOR = led_mask[1];
  PTA->PDOR = led_mask[2];
  /* Enable Output */
  PTD->PDDR = led_mask[0];
  PTC->PDDR = led_mask[1];
  PTA->PDDR = led_mask[2];
}

/*----------------------------------------------------------------------------
  Function that turns on Red LED
 *----------------------------------------------------------------------------*/
void LEDRed_On (void) {
  PTA->PSOR   = led_mask[LED_BLUE];   /* Blue LED Off*/
  PTD->PSOR   = led_mask[LED_GREEN];  /* Green LED Off*/
  PTC->PCOR   = led_mask[LED_RED];    /* Red LED On*/
}

/*----------------------------------------------------------------------------
  Function that turns on Blue LED
 *----------------------------------------------------------------------------*/
void LEDGreen_On (void) {
  PTC->PSOR   = led_mask[LED_RED];    /* Red LED Off*/
  PTA->PSOR   = led_mask[LED_BLUE];   /* Blue LED Off*/
  PTD->PCOR   = led_mask[LED_GREEN];  /* Green LED On*/
}

/*----------------------------------------------------------------------------
  Function that turns on Green LED
 *----------------------------------------------------------------------------*/
void LEDBlue_On (void) {
  PTD->PSOR   = led_mask[LED_GREEN];  /* Green LED Off*/
  PTC->PSOR   = led_mask[LED_RED];    /* Red LED Off*/
  PTA->PCOR   = led_mask[LED_BLUE];   /* Blue LED On*/
}

/*----------------------------------------------------------------------------
 *      Main: Initialize and start RTX Kernel
 *---------------------------------------------------------------------------*/
extern void benchmark(void);
volatile unsigned count;
int main (void) {
	unsigned toggle = 0;
	unsigned n, ack;
	
  SystemCoreClockUpdate();
  LED_Initialize();                         // Initialize the LEDs
	
	SIM->SCGC6 |= SIM_SCGC6_FTM0_MASK;
  FTM0->SC = 0x0;
	FTM0->MOD = 0xFFFF;
  FTM0->CNTIN = 0;
  FTM0->SC = FTM_SC_CLKS(1) | FTM_SC_PS(3);
	
	while(1) {
		// LED activity
		toggle = !toggle;
		if(toggle) LEDRed_On();  else LEDGreen_On();

		// Reset timer and run test
		ack = 0;
		for(n = 0; n < 100; n++) {
			FTM0->CNT = 0x0;
			benchmark();
			ack += FTM0->CNT;
		}
		count = ack / 100;
			
		// BREAKPOINT HERE - check timer
		count = count; // dummy for breakpoint (originally ~0xBB15)
	}
}
